/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file dataToXML.cxx
	\brief class implemenation XML generator

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "mnrSim.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
ShmStruct * shm_struct;
/*<-End Global Variables-->*/

/************************************************************\
  |                  DataToXML_Parser::init                    |
  \************************************************************/
/*!
  \brief the initializing function for the XML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToXML_Parser has been created
  \post the XML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
*/

MNRSimThread::MNRSimThread(String name) : ThreadedPipe(name)
{
	waypoints[0].number   = 0;
	waypoints[0].next     = 0;
	waypoints[0].orbitRad = 5;
	waypoints[0].pos.lat  = 40.15 + (rand() / RAND_MAX - 0.5) * 0.05;
	waypoints[0].pos.lon  = -105.25 + (rand() / RAND_MAX - 0.5) * 0.05; 
	waypoints[0].alt      = 0;

	current_waypoint     = 0;
	gps.pos.lat          = waypoints[current_waypoint].pos.lat;
	gps.pos.lon          = waypoints[current_waypoint].pos.lon;
	gps.alt.value        = 0;
	gps.alt.geoSep       = 0;
	gps.utc.hours        = 0;
	gps.utc.minutes      = 0;
	gps.utc.seconds      = 0;
	gps.accuracy.quality = 0;
	gps.accuracy.sats    = 0;
	gps.accuracy.hdop    = 0;
	gps.track.tru        = 0;
	gps.track.magnetic   = 0;
	gps.sog.value        = 0;
	current_waypoint     = waypoints[current_waypoint].next;
	velocity.x           = 0;
	velocity.y           = -10;
}

bool MNRSimThread::init(ShmStruct * shmStructPtr)
{
	shm_struct = shmStructPtr;
	shmPtr = shmStructPtr;
	mtx = shmPtr->mtx;

	last_telem_tx.stamp();
	return true;
}

void MNRSimThread::update()
{
	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	struct timeval timeout;

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {
		/*<---RX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			if((n = readFifo(INBOARD_RX)) > 0) {
				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					char * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_FP: {
							int num_waypts = cmd.size()/sizeof(Waypt_t);
							if(num_waypts > NUM_WAYPOINTS)
								num_waypts = NUM_WAYPOINTS;

							memcpy(&waypoints,dataPtr,num_waypts*sizeof(Waypt_t));
							current_waypoint = 0;

							// node is way to far away, teleport
							if(GPSRange(gps.pos, waypoints[current_waypoint].pos) > 20000) {
								gps.pos.lat = waypoints[current_waypoint].pos.lat;
								gps.pos.lon = waypoints[current_waypoint].pos.lon;
							}

							CMD_PARAM(fifo_msg.data,0) = CMD_CMD(fifo_msg.data);		
							CMD_CMD(fifo_msg.data)     = CMD_ACK;
							CMD_NUM(fifo_msg.data)     = 1;
							CMD_CSUM(fifo_msg.data)    = 0;
							CMD_CSUM(fifo_msg.data)    = C_calcCheckSum(fifo_msg.data);
							fifo_msg.size              = CMD_SIZE(fifo_msg.data);
							writeFifo(INBOARD_TX);

						} break;
					}
				}
			}
		}
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_telem_tx ) > (TELEMETRY_PERIOD * SEC2MICRO) ) {
				char * dataPtr = (char *)CMD_DATAPTR(fifo_msg.data);

				CMD_CMD((fifo_msg.data)) = CMD_GPS_TELEM;
				CMD_NUM((fifo_msg.data)) = sizeof(GPSStruct_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				//msg_address.s_addr = inet_addr("127.0.0.1");
				//msg_address.s_addr = inet_addr("128.138.75.130");
				//fifo_msg.dest.s_addr = inet_addr("128.138.75.132");
				//fifo_msg.dest.s_addr = inet_addr("224.0.0.1");

				Client * a_client;
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_GPS_TELEM,i)) != NULL)
				{
					i++;
					fifo_msg.dest.s_addr = a_client->address.s_addr;
					writeFifo(INBOARD_TX);
				}
				last_telem_tx.stamp();
				moveMNR();
			}
		}
	}
}

void MNRSimThread::moveMNR()
{
	//float speed = sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
	float speed = 3.0;
	float psides;
	float psidotdes;

	vectorField(gps.pos, atan2(velocity.y, velocity.x), 
	            waypoints[current_waypoint].pos, waypoints[current_waypoint].orbitRad, 
	            speed, &velocity, &psides, &psidotdes);

	gps.pos.lat  += M_TO_LAT(velocity.x);
	gps.pos.lon  += M_TO_LON(velocity.y, gps.pos.lat);
	gps.track.tru = atan2(velocity.y, velocity.x)*180/M_PI;
	gps.sog.value = speed;

	float r = GPSRange( gps.pos, waypoints[current_waypoint].pos);

	if(r < (waypoints[current_waypoint].orbitRad) + speed)
		current_waypoint = waypoints[current_waypoint].next;
}

MNRSimThread::~MNRSimThread()
{
}
